;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;; FILE:   blocks.lisp
;; AUTHOR: Chris MacLellan
;; DESC:   icarus concepts and skills for ex6 in blocks world
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;; Exercise 6 - Blocks World (Task A)

(create-concepts
 ;; perceptual typing concepts
 ((block ?block)
  :percepts    ((block ?block)))

 ((table ?table)
  :percepts    ((table ?table)))

 ;; other concepts
 ((on ?block1 ?block2)
  :percepts    ((block ?block1 xpos ?xpos1 ypos ?ypos1)
		(block ?block2 xpos ?xpos1 ypos ?ypos2 height ?height2))
  :tests       ((= ?ypos1 (+ ?ypos2 ?height2))))

 ((on-table ?block ?table)
  :percepts    ((block ?block xpos ?xpos1 ypos ?ypos1)
		(table ?table xpos ?xpos2 ypos ?ypos2 height ?height2))
  :tests       ((>= ?ypos1 ?ypos2) (<= ?ypos1 (+ ?ypos2 ?height2))))

 ((holding ?block)
  :percepts    ((block ?block)
		(hand ?hand status ?block)))

 ((distinct ?block1 ?block2)
  :percepts    ((block ?block1)
		(block ?block2))
  :tests       ((not (equal ?block1 ?block2))
		(string< ?block1 ?block2)))

)

(create-skills

 ;; unstack
 ((unstack ?block)
  :percepts    ((block ?block))
  :conditions  ((on ?block ?any1)
		(not (on ?any2 ?block))
		(not (holding ?any3)))
  :action      (*grasp-and-raise ?block)
  :effects     ((holding ?block)
		(not (on ?block ?any1))))
  
 ;; pickup
 ((pick-up ?block)
  :percepts    ((block ?block)
		(table ?table))
  :conditions  ((on-table ?block ?table)
		(not (on ?any ?block))
		(not (holding ?any2)))
  :action      (*grasp-and-raise ?block)
  :effects     ((holding ?block)
		(not (on-table ?block ?table))))
 ;; stack 
 ((stack ?block1 ?to)
  :percepts    ((block ?block1)
		(block ?to))
  :conditions  ((holding ?block1)
		(not (on ?any ?to)))
  :action      (*place-and-ungrasp ?block1 ?to)
  :effects     ((not (holding ?block1))
		(on ?block1 ?to)))
 
 ;; putdown FRED 
 ((put-down FRED ?block)
  :percepts    ((block ?block)
		(table ?table))
  :conditions  ((holding ?block))
  :action      (*place-and-ungrasp ?block ?table)
  :effects     ((not (holding ?block))
		(on-table ?block ?table)))

;; make-clear
  ((make-clear ?block)
  :conditions  ((not (on ?any ?block)))
  :subskills   ()
  :effects     ())

 ((make-clear ?block)
  :conditions  ((on ?any1 ?block)
		(not (holding ?any3)))
  :subskills   ((make-clear ?any1)
		(unstack ?any1)
		(put-down FRED ?any1))
  :effects     ((not (on ?any1 ?block))
		(on-table ?any1 ?table)))

 ;; make-three-tower
 ((make-three-tower ?block1 ?block2 ?block3)
  :conditions ((not (holding ?any))
	       (not (on-table ?block1 ?table))
	       (distinct ?block1 ?block2)
	       (distinct ?block2 ?block3)
	       (distinct ?block3 ?block1))
  :subskills  ((make-clear ?block1)
	       (pick-up ?block1)
	       (put-down FRED ?block1)
	       (make-three-tower ?block1 ?block2 ?block3))
  :effects    ((on-table ?block1 ?table)
	       (on ?block2 ?block1)
	       (on ?block3 ?block2)))

 ((make-three-tower ?block1 ?block2 ?block3)
  :conditions ((not (holding ?any))
	       (on-table ?block1 ?table)
	       (not (on ?block2 ?block1))
	       (distinct ?block1 ?block2)
	       (distinct ?block2 ?block3)
	       (distinct ?block3 ?block1))
  :subskills  ((make-clear ?block1)
	       (make-clear ?block2)
	       (pick-up ?block2)
	       (stack ?block2 ?block1)
	       (make-three-tower ?block1 ?block2 ?block3))
  :effects    ((on-table ?block1 ?table)
	       (on ?block2 ?block1)
	       (on ?block3 ?block2)))

 ((make-three-tower ?block1 ?block2 ?block3)
  :conditions ((not (holding ?any))
	       (on-table ?block1 ?table)
	       (on ?block2 ?block1)
	       (distinct ?block1 ?block2)
	       (distinct ?block2 ?block3)
	       (distinct ?block3 ?block1))
  :subskills  ((make-clear ?block2)
	       (make-clear ?block3)
	       (pick-up ?block3)
	       (stack ?block3 ?block2))
  :effects    ((on-table ?block1 ?table)
	       (on ?block2 ?block1)
	       (on ?block3 ?block2)))



)